
#include "struct_typedef.h"
#include "main.h"
#include "cmsis_os.h"
#include "main.h"
#include "stm32f4xx_hal.h"
#include "pid.h"
#include "CAN_Receive.h"
#include "protocol_camp.h"
#include <stdlib.h>
#define MOTOR_SPEED_PID_KP 0.005f
#define MOTOR_SPEED_PID_KI 0.0f
#define MOTOR_SPEED_PID_KD 0.05f

#define MOTOR_SPEED_PID_MAX_OUT  1000.0f
#define MOTOR_SPEED_PID_MAX_IOUT 1000.0f

#define MOTOR_ANGLE_PID_KP 0.05f
#define MOTOR_ANGLE_PID_KI 0.01f
#define MOTOR_ANGLE_PID_KD 0.5f

#define MOTOR_ANGLE_PID_MAX_OUT  1000.0f
#define MOTOR_ANGLE_PID_MAX_IOUT 1000.0f
extern message_info_t message_info;
motor_message_info_t motor_message_info;
const static fp32 motor_speed_pid[3] = { MOTOR_SPEED_PID_KP, MOTOR_SPEED_PID_KI ,MOTOR_SPEED_PID_KD};
const motor_measure_t *motor_measure;
const static fp32 motor_angle_pid[3] = { MOTOR_ANGLE_PID_KP, MOTOR_ANGLE_PID_KI ,MOTOR_ANGLE_PID_KD};
extern motor_info_t motor_info;
pid_type_def motorpid_angle  ;
pid_type_def motorpid_speed  ;
int16_t rpm =400;
int16_t current = 0; 
int ang=0;
int nowang=0;
int lastang=0;
int delang=0;
int tar=100000;
int speed=0;
int16_t gm6020_current = 50000;
extern void rm_queue_data(uint16_t cmd_id,void* buf,uint16_t len ); 

const motor_measure_t *motor_measure;

const motor_measure_t *motor_measure2;
void Motor_task(void const * argument)
{
		// motor_measure = get_chassis_motor_measure_point(0);

		// motor_measure2 = get_yaw_gimbal_motor_measure_point();
		
		PID_init(&motorpid_angle,PID_POSITION,motor_angle_pid,MOTOR_ANGLE_PID_MAX_OUT,MOTOR_ANGLE_PID_MAX_IOUT);
		PID_init(&motorpid_speed,PID_POSITION,motor_speed_pid,MOTOR_SPEED_PID_MAX_OUT,MOTOR_SPEED_PID_MAX_IOUT);
		motor_measure=get_chassis_motor_measure_point(0);
		lastang=motor_measure->ecd;
    while(1)
    {
				
				// if(abs(tar-ang)>=500)
				// {	
				// 	motor_measure=get_chassis_motor_measure_point(0);
				// 	nowang=motor_measure->ecd;
				// 	if((nowang-lastang)>4096)
				// 	{
				// 		delang=nowang-lastang-8191;
				// 	}
				// 	else if ((nowang-lastang)<-4096)
				// 	{
				// 		delang=nowang-lastang+8191;
				// 	}
				// 	else
				// 	{
				// 		delang=nowang-lastang;
				// 	}

				// 	ang+=delang;
				// 	lastang=nowang;
				// 	speed = PID_calc(&motorpid_speed,ang,tar);
				// 	current=PID_calc(&motorpid_angle,delang,speed);
					
				// } test the angle control ,have not perfect ,you can fix it! 
				
					//current = PID_calc(&motorpid_speed,motor_measure->speed_rpm,rpm);
					//motor_can_set_current =PID_calc(&motorpid_speed,motor_measure->speed_rpm,current);
					//CAN_cmd_gimbal(gm6020_current,gm6020_current,0,0);
				
				motor_message_info.motor1_ecd=motor_measure->ecd;
				motor_message_info.motor1_speed_rpm=motor_measure->speed_rpm;
				motor_message_info.motor1_tepm=motor_measure->temperate;
				rm_queue_data(MM_ID,&motor_message_info,sizeof(motor_message_info_t));
			   	CAN_cmd_chassis(motor_info.current1,motor_info.current2,motor_info.current3,motor_info.current4);

				osDelay(50);
			
	 
	
	}		
}
